#include "precomp.h"

#include "Grid.h"
#include "../Entity/Entity.h"

namespace Tmpl {

static int gSize = 0;
static float gfSize = 0;
static GridCell* gCells;
static int* gCollisions;

void Grid::Init(int size)
{
	gSize = size;
	gfSize = float(gSize);
	gCells = new GridCell[size*size];

	gCollisions = MALLOC64(size*size, int);
	for (int i=0; i<size*size; ++i)
	{
		gCollisions[i] = COLLISION_NONE;
	}
}
void Grid::Clear()
{
	delete [] gCells;
	FREE64(gCollisions);
}

int Grid::GetSize()
{
	return gSize;
}
int Grid::GetCellIdFromXY(int x, int y)
{
	return Clamp(0, gSize-1, y) * gSize + Clamp(0, gSize-1, x);
}
GridCell& Grid::GetCellFromXY(int x, int y) 
{
	return gCells[GetCellIdFromXY(x,y)];
}
int Grid::GetCellIdFromPos( const Vec2f& pos)
{
	return GetCellIdFromXY(int(pos.x), (int)pos.y);
}
Vec2f Grid::GetCellCenter(int x, int y)
{
	return Vec2f(float(x) + 0.5f, float(y) + 0.5f);
}
Vec2f Grid::GetCellCenter(int cellId)
{
	const int cellY =  Clamp(0, gSize-1, cellId / gSize);
	const int cellX =  Clamp(0, gSize-1, cellId - cellY * gSize);
	return Vec2f(float(cellX) + 0.5f, float(cellY) + 0.5f);
}
AABB2f Grid::GetCellAABB(int x, int y)
{
	float cellX = float(Clamp(0, gSize-1, x));
	float cellY = float(Clamp(0, gSize-1, y));
	return AABB2f(Vec2f(cellX, cellY),Vec2f(cellX+1.0f, cellY+1.0f));
}

AABB2f Grid::GetCellAABB(int x, int y, float expandRange)
{
	float cellX = float(Clamp(0, gSize-1, x));
	float cellY = float(Clamp(0, gSize-1, y));
	return AABB2f(Vec2f(cellX-expandRange, cellY-expandRange),Vec2f(cellX+expandRange+1.0f, cellY+expandRange+1.0f));
}


// Overlap
Vec2f Grid::SolveOverlap(const Vec2f& pos, float size, Entity** entities, int maxEntities)
{
	Vec2f result = pos;
	bool solved = false;
	while (!solved)
	{
		solved = true;

		// collision on a building
		int collisionId = Grid::GetCollisionIdFromPos(result);
		if (collisionId != Grid::COLLISION_NONE)
		{
			result.x += 1.0f;
			solved = false;
		}

		// collision on entities
		int entCount = Grid::SelectEntitiesInRangeFast(entities, maxEntities, result, size, Entity::ENTITY_ALL);
		for (int i=0; i<entCount; ++i)
		{
			const Entity* ent = entities[i];
			Vec2f vDist = ent->GetPos() - result;
			float len = vDist.Length();
			float overlap = (ent->GetSize() + size) - len;
			if (overlap > len)
			{
				solved = false;
				if (len < 0.0001f)
					result.x += overlap;
				else
					result += (vDist / len) * overlap;
			}
		}
	}

	return result;
}
// Collision
void Grid::SetCollisionId(int cellId, int collisionId)
{
	gCollisions[cellId] = collisionId;
}
int Grid::GetCollisionIdFromPos(const Vec2f& pos)
{
	return gCollisions[GetCellIdFromXY(int(pos.x), (int)pos.y)];
}
int Grid::GetCollisionIdFromXY(int x, int y)
{
	return gCollisions[GetCellIdFromXY(x,y)];
}
int Grid::GetCollisionIdFromCell(int idx)
{
	return gCollisions[idx];
}
// Update Entities
void Grid::AddEntity(Entity* ent, int cellId)
{
	GridCell& cell = gCells[cellId];
	cell.push_back(ent);
}
void Grid::RemoveEntity(Entity* ent, int cellId)
{
	GridCell& cell = gCells[cellId];
	for (GridEntArray::iterator it=cell.begin(); it!=cell.end();  ++it)
	{
		if (*it == ent)
		{
			*it = cell.back();
			cell.pop_back();
			break;
		}
	}
}
void Grid::UpdateEntity(Entity* ent, int oldCellId, int newCellId)
{
	RemoveEntity(ent, oldCellId);
	AddEntity(ent, newCellId);
}
const GridCell& Grid::GetEntitiesInCell(int x, int y)
{
	return gCells[GetCellIdFromXY(x,y)];
}
// Rays
inline float TMax(float rayPos, float rayDir)
{
	if(rayDir > 0.0f)
	{
		return (ceil(rayPos) - rayPos) / rayDir;
	}
	else if(rayDir < 0.0f)
	{
		return (floor(rayPos) - rayPos) / rayDir;
	}
	return 0.0f;
}
float Grid::SphereRayIntersectCollision(const Ray2f& ray, float radius)
{
	// Ray Direction
	const Vec2f dir = ray.GetDir();
	const int stepX = (dir.x < 0.0f ? -1 : 1);
	const int stepY = (dir.y < 0.0f ? -1 : 1);

	// Ray Position
	const Vec2f pos = ray.GetOrigin();
	int iX = (int)pos.x;
	int iY = (int)pos.y;

	// traverse the ray
	const float rayLength = ray.GetLength();
	const float tDeltaX = fabs(1.0f / dir.x);
	const float tDeltaY = fabs(1.0f / dir.y);

	float tMaxX = TMax(pos.x, dir.x);
	float tMaxY = TMax(pos.y, dir.y);

	int iRadius = int(radius) + 1;

	AABB2f aabb;
	float ti = 0.0f;
	do
	{
		// check collision
		for (int y=-iRadius; y<=iRadius; ++y)
		for (int x=-iRadius; x<=iRadius; ++x)
		{
			int collisionId = GetCollisionIdFromXY(iX+x, iY+y);
			if (collisionId > Grid::COLLISION_NONE)
			{
				float fX = float(ClampAround(iX, gSize));
				float fY = float(ClampAround(iY, gSize));
				aabb.min.x = fX - radius;
				aabb.min.y = fY - radius;
				aabb.max.x = fX + radius + 1.0f;
				aabb.max.y = fY + radius + 1.0f;
				float distance = ray.IntersectBox(aabb);
				if (distance >= 0.0f)
				{
					return distance;
				}
			}
			//gCollisions[GetCellIdFromXY(iX+x, iY+y)] = Grid::COLLISION_WATER;
		}
		

		// step it
		if(tMaxX < tMaxY)
		{
			iX += stepX;
			ti = tMaxX;
			tMaxX += tDeltaX;
		}
		else
		{
			iY += stepY;
			ti = tMaxY;
			tMaxY += tDeltaY;
		}
	}
	while(ti <= rayLength);

	return -1.0f;
}

int Grid::RayIntersectCollisionId(const Ray2f& ray, Vec2f& intersectPos)
{
	const Vec2f& dir = ray.GetDir();
	const int stepX = (dir.x < 0.0f ? -1 : 1);
	const int stepY = (dir.y < 0.0f ? -1 : 1);

	const Vec2f& pos = ray.GetOrigin();
	int iX = (int)pos.x;
	int iY = (int)pos.y;

	const float rayLength = ray.GetLength();

	const float tDeltaX = fabs(1.0f / dir.x);
	const float tDeltaY = fabs(1.0f / dir.y);

	float tMaxX = TMax(pos.x, dir.x);
	float tMaxY = TMax(pos.y, dir.y);

	float ti = 0.0f;
	do
	{
		// check collision
		int collisionId = GetCollisionIdFromXY(iX, iY);
		if (collisionId != Grid::COLLISION_NONE)
		{
			intersectPos = pos + dir * ti;
			return collisionId;
		}
		//gCollisions[GetCellIdFromXY(iX, iY)] = Grid::COLLISION_WATER;

		// step it
		if(tMaxX < tMaxY)
		{
			iX += stepX;
			ti = tMaxX;
			tMaxX += tDeltaX;
		}
		else
		{
			iY += stepY;
			ti = tMaxY;
			tMaxY += tDeltaY;
		}
	}
	while(ti <= rayLength);
	return Grid::COLLISION_NONE;
}


int Grid::RayIntersect(const Ray2f& ray, Vec2f& intersectPos)
{
	const Vec2f& dir = ray.GetDir();
	const int stepX = (dir.x < 0.0f ? -1 : 1);
	const int stepY = (dir.y < 0.0f ? -1 : 1);

	const Vec2f& pos = ray.GetOrigin();
	int iX = (int)pos.x;
	int iY = (int)pos.y;

	// traverse the ray
	const float rayLength = ray.GetLength();
	const float tDeltaX = fabs(1.0f / dir.x);
	const float tDeltaY = fabs(1.0f / dir.y);
	float tMaxX = TMax(pos.x, dir.x);
	float tMaxY = TMax(pos.y, dir.y);

	float ti = 0.0f;
	do
	{
		int cellId = GetCellIdFromXY(iX,iY);

		// Collide with buildings
		int collisionId = GetCollisionIdFromCell(cellId);
		if (collisionId != Grid::COLLISION_NONE)
		{
			intersectPos = pos;
			return collisionId;
		}

		// Collide with entities
		GridCell& cell = gCells[cellId];
		for (GridEntArray::const_iterator it = cell.begin(); it != cell.end(); ++it)
		{
			Entity* ent = *it;
			float dist = ray.IntersectSphere(ent->GetPos(), ent->GetSize());
			if (dist >= 0.0f)
			{
				intersectPos = ray.GetPosAlongRay(dist);
				return ent->GetId();
			}
		}

		// step it
		if(tMaxX < tMaxY)
		{
			iX += stepX;
			ti = tMaxX;
			tMaxX += tDeltaX;
		}
		else
		{
			iY += stepY;
			ti = tMaxY;
			tMaxY += tDeltaY;
		}
	}
	while(ti <= rayLength);

	return Grid::COLLISION_NONE;
}

// Queries
int Grid::SelectEntitiesInRangeFast(Entity** entities, int maxEntities, Vec2f pos, float range, int flags)
{
	int entCount = 0;
	int x = int(pos.x);
	int y = int(pos.y);
	int radius = int(range) + 1;
	GridCell* dst1 = gCells + (y-radius) * gSize;
	GridCell* dst2 = gCells + (y+radius) * gSize;
	const GridCell* bufstart = gCells;
	const GridCell* bufend = gCells + gSize*gSize;
	const int sqrrad = radius*radius;
	for(int i=-radius, j=0; i<=0; ++i)
	{
		const int sqry = i*i;
		while( j*j+sqry < sqrrad ){ j++; }
		int xl = x-j; if(xl < 0) xl = 0;
		int xr = x+j; if(xr > gSize-1) xr = gSize-1;
		if( (dst1 > bufstart) && (dst1 < bufend) ) 
		{
			for(int c=xl; c<=xr; ++c)
			{
				GridCell& cell = dst1[c];
				for (GridEntArray::const_iterator it = cell.begin(); it != cell.end(); ++it)
				{
					if (((*it)->GetFlags() & flags) != 0)
					{
						entities[entCount++] = *it;
						if (entCount == maxEntities) return entCount;
					}
				} 
			}
		}
		if( (i != 0) && (dst2 > bufstart) && (dst2 < bufend) )
		{
			for(int c=xl; c<=xr; ++c)
			{
				GridCell& cell = dst2[c];
				for (GridEntArray::const_iterator it = cell.begin(); it != cell.end(); ++it)
				{
					if (((*it)->GetFlags() & flags) != 0)
					{
						entities[entCount++] = *it;
						if (entCount == maxEntities) return entCount;
					}
				} 
			}
		}
		dst1 += gSize, dst2 -= gSize;
	}
	return entCount;
}
int Grid::SelectEntitiesInRange(Entity** entities, int maxEntities, Vec2f pos, float range, int flags)
{
	float sqrRange = range * range;
	int entCount = 0;
	int x = int(pos.x);
	int y = int(pos.y);
	int radius = int(range+0.5f);
	GridCell* dst1 = gCells + (y-radius) * gSize;
	GridCell* dst2 = gCells + (y+radius) * gSize;
	const GridCell* bufstart = gCells;
	const GridCell* bufend = gCells + gSize*gSize;
	const int sqrrad = radius*radius;
	for(int i=-radius, j=0; i<=0; i++)
	{
		const int sqry = i*i;
		while( j*j+sqry < sqrrad ){ j++; }
		int xl = x-j; if(xl < 0) xl = 0;
		int xr = x+j; if(xr > gSize-1) xr = gSize-1;
		if( (dst1 > bufstart) && (dst1 < bufend) ) 
		{
			for(int c=xl; c<=xr; ++c)
			{
				GridCell& cell = dst1[c];
				for (GridEntArray::iterator it = cell.begin(); it != cell.end(); ++it)
				{
					Entity* ent = *it;
					if ((ent->GetFlags() & flags) != 0)
					{
						float size = ent->GetSize();
						Vec2f vdist = (ent->GetPos() - pos);
						if (vdist.SqrLength() < sqrRange + size*size)
						{
							entities[entCount++] = ent;
							if (entCount == maxEntities) return entCount;
						}
					}
				} 
			}
		}
		if( i != 0 && (dst2 > bufstart) && (dst2 < bufend) )
		{
			for(int c=xl; c<=xr; ++c)
			{
				GridCell& cell = dst2[c];
				for (GridEntArray::iterator it = cell.begin(); it != cell.end(); ++it)
				{
					Entity* ent = *it;
					if ((ent->GetFlags() & flags) != 0)
					{
						float size = ent->GetSize();
						Vec2f vdist = (ent->GetPos() - pos);
						if (vdist.SqrLength() < sqrRange + size*size)
						{
							entities[entCount++] = ent;
							if (entCount == maxEntities) return entCount;
						}
					}
				} 
			}
		}
		dst1 += gSize, dst2 -= gSize;
	}
	return entCount;

	/*
	int entCount = 0;

	float sqrRange = range * range;
	int radius = int(range+0.5f) + 1;
	int posX = int(pos.x);
	int posY = int(pos.y);
	int topY = posY - radius;
	int btmY = posY + radius;
	const int sqrrad = radius*radius;
	for(int i=-radius, j=0; i<=0; i++)
	{
		const int sqry = i*i;
		while( j*j+sqry < sqrrad ){ ++j; }
		int xl = posX-j; int xr = posX+j;
		for(int x=xl; x<=xr; ++x)
		{ 
			// Evaluate Top Cell
			GridCell& top = GetCellFromXY(x, topY); 
			for (GridEntArray::const_iterator it = top.begin(); it != top.end(); ++it)
			{
				Entity* ent = *it;
				if ((ent->GetFlags() & flags) != 0)
				{
					float size = ent->GetSize();
					Vec2f vdist = (ent->GetPos() - pos);
					if (vdist.SqrLength() < sqrRange + size*size)
					{
						entities[entCount++] = ent;
						if (entCount == maxEntities) return entCount;
					}
				}
			}

			// Evaluate Bottom Cell
			if (btmY != topY)
			{
				GridCell& btm = GetCellFromXY(x, btmY);
				for (GridEntArray::const_iterator it = btm.begin(); it != btm.end(); ++it)
				{
					Entity* ent = *it;
					if ((ent->GetFlags() & flags) != 0)
					{
						float size = ent->GetSize();
						Vec2f vdist = (ent->GetPos() - pos);
						if (vdist.SqrLength() < sqrRange + size*size)
						{
							entities[entCount++] = ent;
							if (entCount == maxEntities) return entCount;
						}
					}
				}
			}
		}
		++topY;	--btmY;
	}

	return entCount;
	*/
}


}